
#include <cv.h>
#include <highgui.h>
#include <stdlib.h>
#include <stdio.h>
//#include <imgproc.h>
//#include <cvcam.h>
using namespace cv;
using namespace std;
int thresh = 150, N = 101;
const char* wndname = "Capture";

static double angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

static void findSquares( const Mat& image, vector<vector<Point> >& squares )
{
    squares.clear();

    Mat pyr, timg, gray0(image.size(), CV_8U), gray;

    pyrDown(image, pyr, Size(image.cols/2, image.rows/2));
    pyrUp(pyr, timg, image.size());
    vector<vector<Point> > contours;

    for( int c = 0; c < 3; c++ )
    {
        int ch[] = {c, 0};
        mixChannels(&timg, 1, &gray0, 1, ch, 1);

        for( int l = 0; l < N; l++ )
        {
            if( l == 0 )
            {
                Canny(gray0, gray, 0, thresh, 5);
                dilate(gray, gray, Mat(), Point(-1,-1));
            }
            else
            {
                gray = gray0 >= (l+1)*255/N;
            }

            findContours(gray, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

            vector<Point> approx;

            for( size_t i = 0; i < contours.size(); i++ )
            {
                approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.02, true);

                if( approx.size() == 4 &&
                		fabs(contourArea(Mat(approx))) > 1000 &&
                		fabs(contourArea(Mat(approx))) < 10000 &&
                    isContourConvex(Mat(approx)) )
                {
                    double maxCosine = 0;

                    for( int j = 2; j < 5; j++ )
                    {
                        // find the maximum cosine of the angle between joint edges
                        double cosine = fabs(angle(approx[j%4], approx[j-2], approx[j-1]));
                        maxCosine = MAX(maxCosine, cosine);
                    }

                    // if cosines of all angles are small
                    // (all angles are ~90 degree) then write quandrange
                    // vertices to resultant sequence
                    if( maxCosine < 0.07 )
                        squares.push_back(approx);
                }
            }
        }
    }
}


// the function draws all the squares in the image
static void drawSquares( Mat& image, const vector<vector<Point> >& squares )
{
    for( size_t i = 0; i < squares.size(); i++ )
    {
        const Point* p = &squares[i][0];
        int n = (int)squares[i].size();
        polylines(image, &p, &n, 1, true, Scalar(0,255,0), 3, CV_AA);
    }

    imshow(wndname, image);
}
int main(int argc, char* argv[])
{
        CvCapture* capture = cvCreateCameraCapture( 0 );
	    assert( capture );
        double width = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);
        double height = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT);
        printf("[i] %.0f x %.0f\n", width, height );
        IplImage* frame=0;
        cvNamedWindow(wndname, CV_WINDOW_AUTOSIZE);
        cvNamedWindow("debug", CV_WINDOW_AUTOSIZE);
        printf("[i] press Enter for capture image and Esc for quit!\n\n");
        int counter=0;
        char filename[512];
        vector<vector<Point> > squares;
        while(true){
                frame = cvQueryFrame( capture );
                //cvShowImage(wndname, frame);
                Mat pic(frame,1);
                findSquares(pic, squares);
                drawSquares(pic, squares);
                char c = cvWaitKey(33);
                if (c == 27) {
                        break;
                }
                else if(c == int('\n')) {
                        sprintf(filename, "Image%d.jpg", counter);
                        printf("[i] capture... %s\n", filename);
                        cvSaveImage(filename, frame);
                        counter++;
                }
        }
        cvReleaseCapture( &capture );
        cvDestroyWindow("capture");
        cvDestroyWindow("debug");
        return 0;
}
